#!/bin/bash

# 停止所有 ROS 2 节点
ros2 daemon stop

# 强制终止所有 ROS 2 相关进程
pkill -9 -f "ros2"
pkill -9 -f "rclpy"
pkill -9 -f "rclcpp"
pkill -9 -f "/fishbot_bringup"
pkill -9 -f "/fishbot_motion_control"
pkill -9 -f "ydlidar_node"
pkill -9 -f "static_tf_pub_laser"
pkill -9 -f "robotarrawkey"
pkill -9 -f "joint_state_publisher"
pkill -9 -f "robot_state_publisher"
pkill -9 -f "ros2-launch"

# 清理 DDS 缓存
rm -rf /tmp/ros2_* /tmp/cyclonedds_* /tmp/ros/ /var/tmp/cyclonedds_*

# 清除环境变量
unset ROS_DISTRO
unset ROS_VERSION
unset ROS_MASTER_URI
unset ROS_DOMAIN_ID

# 关闭虚拟串口
rm -f /tmp/fishbot_laser

# 重新加载 ROS 2 环境
source /opt/ros/humble/setup.bash
source ~/physical_robot/install/setup.bash

# 启动 ROS 2 守护进程
ros2 daemon start

echo "ROS 2 environment has been restarted."
